/*
 * imu.c
 *
 *  Created on: 2023骞�4鏈�29鏃�
 *      Author: 0
 */
#include "zf_common_headfile.h"

float Yaw = 0, pittch = 0;    // pitch:俯仰角  yaw:偏航角   融合后的角度

// 获取陀螺仪值并进行滤波
void imu_filter()
{
    static float Angle_gz = 0,Angle_gy = 0;

    float acc_x = 0, acc_y = 0, acc_z = 0; //加速度计采集到的原始数据换算单位后

    imu660ra_get_gyro();
    imu660ra_get_acc ();
    if (imu660ra_acc_x < 32764)
    {
        acc_x = imu660ra_acc_x / 4096.0;
    }
    else
    {
        acc_x = 1 - (imu660ra_acc_x - 49152) / 4096.0;
    }
    if (imu660ra_acc_y < 32764)
    {
        acc_y = imu660ra_acc_y / 4096.0;
    }
    else
    {
        acc_y = 1 - (imu660ra_acc_y - 49152) / 4096.0;
    }
    if (imu660ra_acc_z < 32764)
    {
        acc_z = imu660ra_acc_z / 4096.0;
    }
    else
    {
        acc_z = (imu660ra_acc_z - 49152) / 4096.0;
    }
    imu660ra_gyro_z /= 16.4;
    imu660ra_gyro_y /= 16.4;

    Angle_gz = Angle_gz + imu660ra_gyro_z * 0.004;

    Angle_gy = Angle_gy + imu660ra_gyro_y * 0.004;

    pittch = Angle_gy;
    Yaw = Angle_gz;
}
